/*!
* @file    pid.c
* @version v0.1
* @date    2018-02-05
* @author  苏勇 suyong_yq@126.com
* @brief   实现PID算法的处理流程
*/

#include "pid.h"

void PID_Init(PID_Handle_T *handler, float p, float i, float d)
{
    handler->ParamP = p;
    handler->ParamI = i;
    handler->ParamD = d;
    handler->Diff1  = 0.0f;
    handler->Diff2  = 0.0f;
    handler->TargetVal = 0.0f;
    handler->CtrlDeltaVal = 0.0f;
}

void PID_SetTargetValue(PID_Handle_T *handler, float targetVal)
{
    handler->TargetVal = targetVal;
}

/*
 * 增量式PID
 * targetVal值是期望设定值
 * newBackVal值是实际反馈得到的值
 * 返回值将同targetVal在同一个域
 */
float PID_GetDeltaOutput(PID_Handle_T *handler, float newBackVal)
{
    /* 更新差分值 */
    handler->Diff2 += handler->Diff1; /* Diff2累积误差 */
    handler->Diff1 = handler->TargetVal - newBackVal * handler->ParamP; /* 实际采样值换算之后相对于目标值的差距 */

    /* 计算新的控制值:使用误差计算修正的调整量 */
    handler->CtrlDeltaVal = handler->Diff1 * handler->ParamI
                          + handler->Diff2 * handler->ParamD;

    return handler->CtrlDeltaVal;
}

/* 绝对值PID */
float PID_GetAbsOutput(PID_Handle_T *handler, float newBackVal)
{
    return handler->TargetVal + PID_GetDeltaOutput(handler, newBackVal);
}

/* EOF. */

